/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/** 
 * \file
 * \brief Simulator Control Object - interface to simulator provoding funcs such as insert new object.
 * \author Antti Maula <antti.maula@tkk.fi>
 */

#include <libxml/parser.h>
#include <libxml/tree.h>
#include <assert.h>
#include "owndebug.h"
#include "SimCtrlObject.h"
#include "UnitServer.hpp"
#include "MaCICtrlServer.hpp"
#include "World.h"

///////// Units
#include "J2B2.h"
#include "MottiPbV.h"
#include "AGV.h"


#define GIMI_CLIENT_API 20000
#include "gimi.h"


//*****************************************************************************
volatile bool CSimCtrlObject::iSimCtrlObjectActive = false;
//*****************************************************************************
using namespace MaCI;
using namespace MaCI::Unit;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CObject* CSimCtrlObject::LoadMLMA(CWorld *aWorld, xmlNode *aNode)
{
  CObject *object = NULL;

  assert(aWorld != NULL);
  assert(aNode != NULL);
  
  // Parameters for position
  xmlChar *macigroup = xmlGetProp(aNode, (xmlChar *) "MaCIGroup");
  
  // Process.
  if (macigroup) {
    std::string group = reinterpret_cast<const char*>(macigroup);
    object = new CSimCtrlObject(aWorld, group);
    
  } else {
    dPrint(1,"MLMA Parse Error: Not enough parameter for SimCtrlObject");
    
  }
  
  // Cleanup (Always do this to clean out properties which were found)
  if (macigroup) xmlFree(macigroup);
  
  
  if (object){
    aWorld->InsertObject(object);
  }
  
  return object;
}
//*****************************************************************************

CSimCtrlObject::CSimCtrlObject(CWorld *aWorld, 
                               const std::string &aMaCIGroup)
  : CSync(1,1),
    CObject(aWorld, NULL),
    CThread(1),
    iMaCIGroup(aMaCIGroup),
    iObjectArray(),
    iGIMIPtr(NULL),
    iGIMIBufferId(-1),
    iMaCICtrlServer(NULL),
    iUnitServer(NULL)
{
  Lock();
  if (iSimCtrlObjectActive == false) {
    iSimCtrlObjectActive = true;
    dPrint(ODTEST,"Constructed instance %p of SimCtrlObject", this);

  } else {
    dPrintLCRed(ODERROR,"Only one SimCtrlObject may be active inside one FSRSim instance!");
    abort();

  }
  Unlock();
}
//*****************************************************************************
    
CSimCtrlObject::~CSimCtrlObject()
{
  if (IsThreadRunning()) SetRequestTermination();

  if (iGIMIPtr) {
    iGIMIPtr->stop();
    delete iGIMIPtr;
  }

  if (IsThreadRunning()) WaitThread();
}
//*****************************************************************************

void CSimCtrlObject::Activate()
{

  assert(IsThreadRunning() == false);

  if (!iGIMIPtr) {
    iGIMIPtr = new gimi::GIMI(100, 
                              "SimCtrlObject providing interface to control some FSRSim internals", 
                              "", 
                              0, 
                              0);
    assert(iGIMIPtr);
    
    // Call SimWorld connector.
    if ( CObject::SimWorld->ConnectGimi(iGIMIPtr, "") ) {
      iGIMIPtr->addAcceptedService(GIMI_PROTOCOL_FSRSIM_SIMCTRLOBJECT,0,"SimCtrlObject commands");

      // Construct services.

      // MaCICtrlServer...
      iMaCICtrlServer = new MaCI::MaCICtrl::CMaCICtrlServer(iGIMIPtr);
      iMaCICtrlServer->SetGroupName(iMaCIGroup);
      MaCI::EMaCIError e = iMaCICtrlServer->Open();
      assert(e == MaCI::KMaCIOK);


      // Unit server.
      iUnitServer = new MaCI::Unit::CUnitServer(iMaCICtrlServer, 0, 
                                                "FSRSimulator World Unit Ctrl Interface",
                                                "World");

      // Open()s
      if (iUnitServer->Open() == MaCI::KMaCIOK) {
        dPrint(ODTEST,"Unit interface %p succesfully Open():ed", iUnitServer);

        // Last but not least - Register a few available types.
        iUnitServer->RegisterAvailableUnit(KUnitTypeMachine, "MottiPbV");
        iUnitServer->RegisterAvailableUnit(KUnitTypeMachine, "J2B2");
        iUnitServer->RegisterAvailableUnit(KUnitTypeMachine, "AGV");
        iUnitServer->RegisterAvailableUnit(KUnitTypeHexahedron, "Box");



        // Give control to own thread
        RunThread();
        
      } else {
        dPrintLCRed(ODERROR,"Failed to Open() CUnitServer %p! Fatal error!", iUnitServer);
        exit(1);
        
      }

    } else {
      dPrintLCRed(ODERROR,"Failed to activate GIMI instance for SimCtrlObject %p!");
    }
  } else {
    dPrint(ODTEST,"Already activated!");
  }
}
//*****************************************************************************

void CSimCtrlObject::ConstructSimRecipe(const SSimObjectConstructionRecipe &aRecipe)
{
  bool result = true;

  if (aRecipe.implementation == "J2B2") {
    // J2B2
    const std::string &name = MaCI::GetStaticClientName("FSRSim.J2B2."+aRecipe.instance);
    CObject *newobj = new CJ2B2(SimWorld,
                                aRecipe.pose.x_pos,
                                aRecipe.pose.y_pos,
                                aRecipe.pose.z_angle,
                                name,
                                aRecipe.instance,
                                false);
    
    newobj->Activate();

    SimWorld->InsertObject(newobj);

    dPrint(ODTEST,"SimCtrl: Inserted J2B2(%p) at x: %g, y: %g, heading: %g (MaCIGroup set to: '%s', name to: '%s')",
           newobj, aRecipe.pose.x_pos, aRecipe.pose.y_pos, aRecipe.pose.z_angle, aRecipe.instance.c_str(), name.c_str());

    // Update UserPtr
    result = iUnitServer->UpdateActiveUnitUserPtr(aRecipe.active_id, newobj);
    assert(result == true);
    
  } else if (aRecipe.implementation == "MottiPbV") {
    // MottiPbV
    const std::string &name = MaCI::GetStaticClientName("FSRSim.MottiPbV."+aRecipe.instance);

    CObject *newobj = new CMottiPbV(SimWorld,
                                    aRecipe.pose.x_pos,
                                    aRecipe.pose.y_pos,
                                    aRecipe.pose.z_pos,
                                    aRecipe.pose.z_angle,
                                    name,
                                    aRecipe.instance);
    
    newobj->Activate();
    
    SimWorld->InsertObject(newobj);

    dPrint(ODTEST,"SimCtrl: Inserted MottiPbV(%p) at x: %g, y: %g, heading: %g (MaCIGroup set to: '%s', name to: '%s')",
           newobj, aRecipe.pose.x_pos, aRecipe.pose.y_pos, aRecipe.pose.z_angle, aRecipe.instance.c_str(), name.c_str());
    
    // Update UserPtr
    result = iUnitServer->UpdateActiveUnitUserPtr(aRecipe.active_id, newobj);
    assert(result == true);
    
  } else if (aRecipe.implementation == "AGV") {
    // AGV
    const std::string &name = MaCI::GetStaticClientName("FSRSim.AGV."+aRecipe.instance);
    CObject *newobj = new CAGV(SimWorld,
                               aRecipe.pose.x_pos,
                               aRecipe.pose.y_pos,
                               aRecipe.pose.z_angle,
                               name,
                               aRecipe.instance);
    
    newobj->Activate();
    
    SimWorld->InsertObject(newobj);
    
    dPrint(ODTEST,"SimCtrl: Inserted AGV(%p) at x: %g, y: %g, heading: %g (MaCIGroup set to: '%s', name to: '%s')",
           newobj, aRecipe.pose.x_pos, aRecipe.pose.y_pos, aRecipe.pose.z_angle, aRecipe.instance.c_str(), name.c_str());
    
    // Update UserPtr
    result = iUnitServer->UpdateActiveUnitUserPtr(aRecipe.active_id, newobj);
    assert(result == true);
    
  } else {
    dPrintLCRed(ODTEST,"Internal error! Unsupported Implementation although registered!");
    abort();
    
  }

}
//*****************************************************************************

void CSimCtrlObject::ConstructPendingSimRecipes(void)
{
  Lock();
  if (iSimObjectConstructionRecipeArray.size()) {
    for(EACH_IN_i(iSimObjectConstructionRecipeArray)) {
      const SSimObjectConstructionRecipe &socr = *i;
      ConstructSimRecipe(socr);
    }
    iSimObjectConstructionRecipeArray.clear();
  }
  Unlock();
}
//*****************************************************************************

void CSimCtrlObject::Update(float StepSize)
{
  ConstructPendingSimRecipes();
}
//*****************************************************************************
    
void CSimCtrlObject::OnCollision(CPrimitive *, 
                                 int, 
                                 dContact *,
                                 CPrimitive *)
{
}
//*****************************************************************************

void CSimCtrlObject::Draw()
{
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

void CSimCtrlObject::ProcessCommand(const TCommand *aCommand,
                                    const TUnitId *aUnitId,
                                    const TUnitPose *aUnitPose,
                                    const SMessageSource &aMessageSource)
{
  assert(aCommand != NULL);
  assert(iUnitServer != NULL);
  dPrint(ODTEST,"ProcessCommand!");


  uint32_t active_id = 0;
  CUnitData reply;
  unsigned int flags = KCommandFlagReply;
  reply.CreateInternalBinBag();
  reply.SetTimestamp(Common::TTimestamp());

  Lock();
    
    
  if (aCommand->cmd == KCommandCreateUnit && aUnitId && aUnitPose) {
    dPrint(ODTEST,"Command was CreateUnit and UnitId and UnitPose were present");

    const TAvailableUnit *availableunit = iUnitServer->GetAvailableUnitByAvailableId(aUnitId->available_id);
   
    // CHECK that an available unit matching the ID was found.
    if (availableunit) {
      const std::string implementation = availableunit->implementation;
      const std::string instance = aUnitId->instance;
      const EUnitType &type = (EUnitType)availableunit->unit_type;

      // Register Active.
      active_id = iUnitServer->RegisterActiveUnit(aUnitId->available_id,
                                                  aUnitId->instance, NULL);

      if (active_id) {
        // Insert element to SimObjectConstructionRecipeArray x-D
        iSimObjectConstructionRecipeArray.push_back(SSimObjectConstructionRecipe(type, implementation, 
                                                                                 instance, *aUnitPose,
                                                                                 active_id));

      } else {
        dPrint(ODTEST,"Activation failed for %u:'%s'! Ignoring", 
               aUnitId->available_id,
               aUnitId->instance);
      }

    } else {
      dPrint(ODERROR,"No AvailableUnit found with given available_id! (Unit: %u)",
             aUnitId->available_id);
      
    }
    
    // Now; Check whether the ActiveID was processed to something meaningful.
    if (active_id) {
      flags |= KCommandFlagResultOK;
      
    } else {
      flags |= KCommandFlagResultError;
      
    }
    
    reply.SetUnitId(TUnitId(aUnitId->available_id, 
                            active_id,
                            aUnitId->instance));    
    
  } else if (aCommand->cmd == KCommandDeleteUnit && aUnitId) {
    dPrint(ODTEST,"DELETE!");
    // Delete given unit

    const MaCI::Unit::TActiveUnitUserPtrPair activeunit = iUnitServer->GetActiveUnitByActiveId(aUnitId->active_id);    
    reply.SetUnitId(TUnitId(0, aUnitId->active_id, ""));    

    if (activeunit.first) {
      dPrint(ODTEST,"Attempting to destroy unit %p (userptr: %p)", activeunit.first, activeunit.second);
      assert(activeunit.second != NULL);
      CObject *object = (CObject*)activeunit.second;
      
      
      // Deactivate
      object->Deactivate();

      // Remove from simulator
      SimWorld->RemoveObject(object);

      // Delete (FIXME!)
      dPrint(ODTEST,"Warning: Not deleting object %p, as it would crash the Sim, please Fix me!", object);
      //delete object;
      
      // Unregister
      iUnitServer->UnregisterActiveUnit(aUnitId->active_id);

      flags |= KCommandFlagResultOK;

    } else{
      dPrint(ODTEST,"No such active_id found (%u)! Cannot destroy.", aUnitId->active_id);
      flags |= KCommandFlagResultError;
    }
    
    
  } else {
    dPrint(ODERROR,"Unsupported command / parameter combination received - Ignoring!");
    flags |= KCommandFlagResultError;

  }

  Unlock();


  reply.SetCommand(TCommand(aCommand->cmd, flags));
  
  // Sending reply
  iUnitServer->SendReply(reply, aMessageSource);

}
//*****************************************************************************

int CSimCtrlObject::ThreadFunction(int aThreadNumber)
{
  assert(aThreadNumber == 0);
  CUnitData input;
  SMessageSource ms;
  EMaCIError e;

  // Now, run in a processing loop waiting for messages to fall in.
  do {
    input.Reset();
    e = iUnitServer->WaitCommand(input, ms, 1000*60*60*24);
    
    dPrint(ODTEST,"WaitCommand returned with %u", e);

    if (e == KMaCIOK) {
      dPrint(ODTEST,"Got Command.");
      const TCommand *cmd = input.GetCommand();
      const TUnitId *unitid = input.GetUnitId();
      const TUnitPose *unitpose = input.GetUnitPose();

      dPrint(ODTEST,"%p %p %p", cmd, unitid, unitpose);
      
      // Now; call process to take care of
      if (cmd) {
        ProcessCommand(cmd, unitid, unitpose, ms);
      }
    }
  } while(!CThread::IsRequestTermination());
  
  return 0;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
